#include "Simple_PID.h"
#include "Simple_time.h"
#include "foc_utils.h"
// PID controller function
float Simple_PIDController (SimplePID_t *handle, float error)
{
    // calculate the time from the last call
    unsigned long timestamp_now = Simple_micros();
    float Ts = (timestamp_now - handle->inside.timestamp_prev) * 1e-6f;
    // quick fix for strange cases (micros overflow)
    if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;

    // u(s) = (P + I/s + Ds)e(s)
    // Discrete implementations
    // proportional part
    // u_p  = P *e(k)
    float proportional = handle->P * error;
    // Tustin transform of the integral part
    // u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
    float integral = handle->inside.integral_prev + handle->I*Ts*0.5f*(error + handle->inside.error_prev);
    // antiwindup - limit the output
    integral = _constrain(integral, -handle->limit, handle->limit);
    // Discrete derivation
    // u_dk = D(ek - ek_1)/Ts
    float derivative = handle->D*(error - handle->inside.error_prev)/Ts;

    // sum all the components
    float output = proportional + integral + derivative;
    // antiwindup - limit the output variable
    output = _constrain(output, -handle->limit, handle->limit);

    // if output ramp defined
    if(handle->output_ramp > 0){
        // limit the acceleration by ramping the output
        float output_rate = (output - handle->inside.output_prev)/Ts;
        if (output_rate > handle->output_ramp)
            output =handle->inside.output_prev + handle->output_ramp*Ts;
        else if (output_rate < -handle->output_ramp)
            output = handle->inside.output_prev - handle->output_ramp*Ts;
    }
    // saving for the next pass
    handle->inside.integral_prev = integral;
    handle->inside.output_prev = output;
    handle->inside.error_prev = error;
    handle->inside.timestamp_prev = timestamp_now;
    return output;
}
